{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix\n",
    "from robot import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import almath\n",
    "import math\n",
    "import copy\n",
    "import motion\n",
    "import time\n",
    "import cv2\n",
    "from nao import Nao"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 运动到指定位置\n",
    "---\n",
    "起始姿态: 见仿真的 cooperation.wbt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "red = Nao(\"127.0.0.1\",9559)\n",
    "blue = Nao(\"127.0.0.1\",9560)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "red.simulationInitPose()\n",
    "blue.simulationInitPose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 其他规律讨论"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 左右臂对称的规律"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=0.0773332, z=0.026255, wx=-1.5651, wy=-0.000355477, wz=-0.254609)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getLArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=-0.0773332, z=0.026255, wx=1.5651, wy=-0.000355378, wz=0.254609)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getRArmPos()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "由上两 cell 可知, 若对称则将y,wx,wz取反即可"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.786008775234, \\quad 0.0466811098158, \\quad -1.17838895321, \\quad -0.835045993328, \\quad -0.29978209734, \\quad 0.0\\right ]$$"
      ],
      "text/plain": [
       "[0.786008775234, 0.0466811098158, -1.17838895321, -0.835045993328, -0.29978209\n",
       "734, 0.0]"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getLArmAngles()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.78600859642, \\quad -0.0466811098158, \\quad 1.178388834, \\quad 0.83504575491, \\quad 0.299782186747, \\quad 0.0\\right ]$$"
      ],
      "text/plain": [
       "[0.78600859642, -0.0466811098158, 1.178388834, 0.83504575491, 0.299782186747, \n",
       "0.0]"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getRArmAngles()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "由上两 cell 可知, 若对称则将序号为 1,2,3,4 的关节取反,维持序号为 0,5 的结果"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### World Frame 初始坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#red.frame = motion.FRAME_ROBOT\n",
    "#red.useSensorValues = False\n",
    "#red.getLArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#red.useSensorValues = True\n",
    "#red.getLArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 使用 motion.AXIS_MASK_ROT 来保证转动姿态的正确\n",
    "# 使桌子能抬平\n",
    "#red.axisMask=motion.AXIS_MASK_ROT\n",
    "#worldPos = red.getLArmPos()\n",
    "#worldPos.wx=-math.pi/2\n",
    "#worldPos.wy=0\n",
    "#worldPos.wz=0\n",
    "#red.setLArmPos(worldPos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#red.getLArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 使得桌子运动到指定位姿\n",
    "#red.axisMask=motion.AXIS_MASK_VEL\n",
    "#worldPos = red.getLArmPos()\n",
    "#worldPos.x=0.18\n",
    "#red.setLArmPos(worldPos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#red.getLArmPos()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 协作运动\n",
    "\n",
    "Nao的每个手臂只有5个自由度,也就是说手臂都不能到达Tson坐标系下任何位姿,\n",
    "仅能达到一个可行解,但对于双臂协作来说,实际上协作意味着\n",
    "LArm能到达RArm坐标系下表示的任意姿态,因为每个臂都至少有一个球面副,\n",
    "换句话说,对任何LArm可到达的位姿,保持RArm与LArm相对位姿不变,\n",
    "则RArm必然也是可达位姿"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "red.simulationInitPose()\n",
    "blue.simulationInitPose()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 单机器人两个机械臂的协同"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.873126, -0.00273188, 0.487486, 0.0389553\n",
       "           0.00273172, -0.999941, -0.0104964, -0.000838798\n",
       "           0.487486, 0.0104964, -0.873067, -0.149678])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 相对矩阵求解\n",
    "BL_Tf = red.getLArmTf()\n",
    "LB_Tf = BL_Tf.inverse()\n",
    "BR_Tf = red.getRArmTf()\n",
    "LR_Tf = LB_Tf*BR_Tf\n",
    "LR_Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=0.0773332, z=0.0262549, wx=-1.5651, wy=-0.000355402, wz=-0.254609)"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getLArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=-0.0773332, z=0.026255, wx=1.5651, wy=-0.000355505, wz=0.254609)"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "red.getRArmPos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 移动主臂 LArm\n",
    "moveTf = almath.Transform.fromPosition(0.02, 0.02, 0) # 相对运动\n",
    "New_BL_Tf = moveTf*BL_Tf\n",
    "red.setLArmTf(New_BL_Tf)\n",
    "time.sleep(0.5)\n",
    "Real_New_BL_Tf = red.getLArmTf()\n",
    "New_BR_Tf = Real_New_BL_Tf*LR_Tf\n",
    "red.setRArmTf(New_BR_Tf)\n",
    "time.sleep(0.5)\n",
    "Real_New_BR_Tf = red.getRArmTf()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([1, 5.78666e-010, -4.50804e-009, -1.9218e-009\n",
       "           5.78666e-010, 1, 1.10298e-009, 2.48205e-011\n",
       "           -4.50804e-009, 1.10298e-009, 1, 1.37221e-009])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Real_New_LB_Tf = Real_New_BR_Tf.inverse()\n",
    "Real_New_LR_Tf = Real_New_LB_Tf*Real_New_BR_Tf\n",
    "Real_New_LR_Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.154666379094$$"
      ],
      "text/plain": [
       "0.154666379094"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "LR_Tf.distance(Real_New_LR_Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.967762, 0.00177865, 0.251861, 0.18056\n",
       "           -0.251867, 0.00542316, 0.967747, 0.0773332\n",
       "           0.000355402, -0.999984, 0.00569631, 0.0262549])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "BL_Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.967921, 0.00093619, 0.251252, 0.185827\n",
       "           -0.251253, 0.00592833, 0.967903, 0.0903878\n",
       "           -0.000583364, -0.999982, 0.00597338, 0.0370154])"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Real_New_BL_Tf"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 不是我算的不对,是API不给力,怎么办?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
